/*******************************************************************************
*   Copyright (C) Harbin Institute of Technology, Shenzhen.
*				  All Rights Reserved.
*   Department: Decoration
********************************************************************************
* File Name   : algorithm_if.h
* Author      : Dayuan
* Version     : v0.01
* Date        : 2019/9/6
* Description : Decoration Rototics Manipulator task.
*******************************************************************************/
/*-History----------------------------------------------------------------------
* Version   Date      Name        Changes and comments
* v0.01     2019/9/6  Dayuan    initial version
*=============================================================================*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef _ALGORITHM_IF_H_
#define _ALGORITHM_IF_H_

#ifdef __cplusplus
 extern "C" {
#endif
     
/**** Includes ****/
#include <stdint.h>
     
/**** Definition of constants ****/
#define PI 3.14159265354f
#define CONTROL_CYCLE 0.001f
#define TO_RADIUS   PI / 180

#define CURRENT 1	
#define LAST 0

/**** Definition of types ****/ 
typedef struct 
{
  double ratio;
	float in;
  float out;
}FILTER_INFO;

typedef struct  
{
	float kp;                 /* kp of force loop */
	float ki;                 /* ki of force loop */
	float kd;                 /* kd of force loop */
	float k_of_im;            /* k of impedance loop */
	float d_of_im;
	FILTER_INFO im_fil;       /* output of impedance control */
	FILTER_INFO gravity;      /* gravity compensate of gb*/
	float speed_des[2];
	float speed_act;
	FILTER_INFO speed_fil;
	float inc[2];             		/* angle of joint from incremental encoder */
	float angle_des;          /* desired angle */
	float angle_error[2];			/*	angle error	*/
	float angle_err_x2[2];		/*	error differential	*/
	float angle_err_int;			/*	error integration	*/
	FILTER_INFO angle_fil;
	float f_des;
	float f_err[2];           /* position error */
	float f_err_x2[2];        /* error of position error */
	float f_act;              /* actual force */
	float f_act_switch;				/* for limit */
	float f_out;
	FILTER_INFO f_fil;
	float pid_out[2];         /* output of pid controller */
	int pwm;                  /* pwm output of each motor */
	uint16_t pose;
	unsigned char error_code;
	unsigned char status;
	float im_offset;
}JOINT_INFO;

/**** Definition of macros ****/
#define PID_DEBUG

/**** Declaration of constants ****/


/**** Declaration of variables ****/
extern JOINT_INFO joint_info;

/**** Declaration of functions ****/
extern void init_1th_filter(FILTER_INFO *filter, float cut_off, float value);
extern void filter_create(JOINT_INFO *joint);
extern float update_1th_filter(FILTER_INFO *filter, float value);

extern void impadence_control(JOINT_INFO *joint);
extern void pid_control(JOINT_INFO *joint);
extern void save_joint_info(JOINT_INFO *joint);

extern void pid_speed_control(JOINT_INFO * joint);
extern void pid_position_control(JOINT_INFO * joint);

extern void smc_controller(JOINT_INFO *joint);
extern void reset_pid_error(JOINT_INFO *joint);

extern void pid_reset(JOINT_INFO *joint);
#ifdef PID_DEBUG
extern void desired_generator(JOINT_INFO *joint,uint8_t clear);
#endif
static float derivate_catch(FILTER_INFO* data);
#ifdef __cplusplus
}
#endif

#endif
